#ifndef	__KALMAN_H
#define	__KALMAN_H
#include "my_def.h"

typedef struct {
	_iq adc_old;
	_iq kal_old;
	_iq Q;
	_iq R;
	_iq Kg;
	_iq P_k1_k1;
	_iq P_k_k1;
	_iq x_k1_k1;
	_iq x_k_k1;
	_iq Z_k;
	_iq In;
	_iq Out;
}KAL_CAL;

extern  KAL_CAL	kal1,kal2;
	
#define KAL_DEFAULT {	\
	_IQ(0),		\
	_IQ(0),		\
	_IQ(0.0001),		\
	_IQ(0.005),		\
	_IQ(0),		\
	_IQ(1),		\
	_IQ(1),		\
	_IQ(0),	\
	_IQ(0),	\
	_IQ(0),	\
	_IQ(0),	\
	_IQ(0),	\
}


#define KAL_FILTER_MACRO(v)																			\
																									\
		v.adc_old	= v.adc_old;																	\
    v.Z_k = v.In;																					\
    v.x_k1_k1 = v.kal_old;																			\
																									\
    v.x_k_k1 = v.x_k1_k1;																			\
    v.P_k_k1 = v.P_k1_k1 + v.Q;																		\
																									\
    v.Kg = _IQdiv(v.P_k_k1,(v.P_k_k1 + v.R)) ;														\
																									\
    v.Out = v.x_k_k1 + _IQmpy(v.Kg , (v.Z_k - v.kal_old));											\
    v.P_k1_k1 = _IQmpy( (_IQ(1) - v.Kg), v.P_k_k1 );												\
    v.P_k_k1 = v.P_k1_k1;																			\
																									\
    v.adc_old = v.In;																				\
    v.kal_old = v.Out;																				
		
		

#endif
